#pragma once
#include "point.hpp"
#include "map.hpp"
#include "path.hpp"
#include "scheduler.hpp"
#include "agent.hpp"
#include "agent_manager.hpp"

#include <string>
#include <vector>
#include <chrono>
#include <iostream>

namespace dw 
{
    class World
    {
        Map map;
        AgentManager agent_manager;
        Scheduler scheduler;
        
        std::chrono::system_clock::time_point timeThen;
        std::chrono::system_clock::time_point timeNow;

    public:
        World() : 
            map(),
            agent_manager()
        {
            timeThen = std::chrono::system_clock::now();
            timeNow = std::chrono::system_clock::now();
        }
        
        World(const std::string& path) : 
            agent_manager()
        {
            map.load(path);
            scheduler.setMap(map.getMapState());
            timeThen = std::chrono::system_clock::now();
            timeNow = std::chrono::system_clock::now();
        }

        ~World() { /* done */ }
    
    public:
        /* Timer */
        double duration()
        {
            timeNow = std::chrono::system_clock::now();
            double dt = std::chrono::duration<double>(timeNow - timeThen).count();
            timeThen = timeNow;
            return dt;
        }

    public:
        /* Map */
        const Map &getMap() const
        {
            return this->map;
        }

        Map &getMap()
        {
            return this->map;
        }

        bool setMap(const Map& m) {
            map = m;
            return true;
        }

        bool decodeMap(const std::string& encoded) {
            map.decode(encoded);
            return true;
        }

    public:
        /* Agent */
        std::shared_ptr<AutonomousAgent> createAutonomousAgent()
        {
            std::shared_ptr<AutonomousAgent> agent = agent_manager.CreateAutonomousAgent();

            while (true)
            {
                Coord pos = map.getSpawnPoint();
                Coord destination = map.getSpawnPoint();

                Coord distance = destination - pos;
                while (distance.mag2() < 0.1)
                {
                    pos = map.getSpawnPoint();
                    destination = map.getSpawnPoint();
                    distance = destination - pos;
                }
    
                std::shared_ptr<Path> p = scheduler.FindPath(agent->getID(), pos, destination);
                if (nullptr != p) {
                    agent->setPosition(pos);
                    agent->follow(p);
                    break;
                }
            }
            return agent;
        }

        std::shared_ptr<Agent> createAgent()
        {
            Coord pos = map.getSpawnPoint();

            std::shared_ptr<Agent> agent = agent_manager.CreateAgent();
            agent->setPosition(pos);
            return agent;
        }

        bool setAgentState(const Agent::State& state, ID id = 0) {
            return agent_manager.setAgentState(state, id);
        }

        bool updateAgentState(const Agent::State& state, ID id = 0) {
            return agent_manager.UpdateAgentState(state, id);
        }

        bool deleteAgent(ID id)
        {
            agent_manager.DeleteAgent(id);
            scheduler.DeletePath(id);
            return true;
        }

        void Update() {
            agent_manager.Update(duration());
        }

        std::unordered_map<ID, std::shared_ptr<Agent>>::iterator agent_begin()
        {
            return agent_manager.begin();
        }

        std::unordered_map<ID, std::shared_ptr<Agent>>::const_iterator agent_begin() const
        {
            return agent_manager.begin();
        }

        std::unordered_map<ID, std::shared_ptr<Agent>>::iterator agent_end()
        {
            return agent_manager.end();
        }

        std::unordered_map<ID, std::shared_ptr<Agent>>::const_iterator agent_end() const
        {
            return agent_manager.end();
        }

    public:
        /* Path */
        std::shared_ptr<Path> getPath(ID id)
        {
            return scheduler.getPath(id);
        }

        std::shared_ptr<Path> FindPath(const Agent::State &state)
        {
            Coord destination = map.getSpawnPoint();
            return scheduler.FindPath(state, destination);
        }

        std::shared_ptr<Path> FindPath(const Agent::State &state, const Coord &destination)
        {
            return scheduler.FindPath(state, destination);
        }

        std::shared_ptr<Path> FindPath(const Agent::State &state, const Coord &start, const Coord &end)
        {
            return scheduler.FindPath(state.id, start, end);
        }

        std::shared_ptr<Path>& simplify(std::shared_ptr<Path>& p)
        {
            p->simplify(map.getMapState());
            return p;
        }

        std::shared_ptr<Path> simplify(ID id) {
            if (!scheduler.Exists(id)) return nullptr;
            std::shared_ptr<Path> p = scheduler.getPath(id);
            p->simplify(map.getMapState());
            return p;
        }

        std::unordered_map<ID, std::shared_ptr<Path>>::iterator path_begin()
        {
            return scheduler.begin();
        }

        std::unordered_map<ID, std::shared_ptr<Path>>::const_iterator path_begin() const
        {
            return scheduler.begin();
        }

        std::unordered_map<ID, std::shared_ptr<Path>>::iterator path_end()
        {
            return scheduler.end();
        }

        std::unordered_map<ID, std::shared_ptr<Path>>::const_iterator path_end() const
        {
            return scheduler.end();
        }

    public:
        friend void to_json(nlohmann::json &j, const World &world);
        friend void from_json(const nlohmann::json &j, World &World);

        std::string encode() {
            nlohmann::json j;
            to_json(j, *this);
            return j.dump();
        }
        
        World& decode(const std::string &data_msg) {
            nlohmann::json j = nlohmann::json::parse(data_msg);
            from_json(j, *this);
            return *this;
        }

        friend std::ostream& operator << (std::ostream& os, World& world) {
            os << world.map.encode() << std::endl;
            os << world.agent_manager;
            return os;
        }
    };

    void to_json(nlohmann::json &j, const World &world)
    {
        nlohmann::json &map = j["map"];
        nlohmann::json &paths = j["paths"];
        nlohmann::json &agents = j["agents"];

        to_json(map, world.map);
        to_json(agents, world.agent_manager);
        to_json(paths, world.scheduler);
    }

    void from_json(const nlohmann::json &j, World &world)
    {
        nlohmann::json map = j["map"];
        nlohmann::json agents = j["agents"];
        nlohmann::json paths = j["paths"];

        from_json(map, world.map);
        from_json(agents, world.agent_manager);
        from_json(paths, world.scheduler);
    }
}